import ctypes
import logging
import os
import sys
import time

logging.basicConfig(filename='tool.log', level=logging.DEBUG, format='%(asctime)s %(levelname)s: %(message)s')


#   调用结构体 POSE
class POSE(ctypes.Structure):
    _fields_ = [("px", ctypes.c_float),
                ("py", ctypes.c_float),
                ("pz", ctypes.c_float),
                ("rx", ctypes.c_float),
                ("ry", ctypes.c_float),
                ("rz", ctypes.c_float)]


class FRAME(ctypes.Structure):
    _fields_ = [("frame_name", ctypes.c_char * 10),
                ("pose", POSE),
                ("payload", ctypes.c_float),
                ("x", ctypes.c_float),
                ("y", ctypes.c_float),
                ("z", ctypes.c_float)]


class Arm:
    def __init__(self, ip=''):
        self.ip = ip
        CUR_PATH = os.path.dirname(os.path.realpath(__file__))
        dllPath = os.path.join(CUR_PATH, "RM_Base.dll")
        self.pDll = ctypes.cdll.LoadLibrary(dllPath)
        #   API 初始化
        self.pDll.RM_API_Init(65, 0)

        #   连接机械臂
        byteIP = bytes(self.ip, "gbk")
        self.nSocket = self.pDll.Arm_Socket_Start(byteIP, 8080, 2000)
        logging.info(f'连接机械臂完毕')
        self.ret = self.pDll.Arm_Socket_State(self.nSocket)
        logging.info(f'连接状态：{self.ret}')


    def send_file(self):
        #   下发机械臂在线编程轨迹文件
        self.pDll.Send_TrajectoryFile.argtypes = (ctypes.c_int, ctypes.c_char_p, ctypes.c_int, ctypes.c_int)
        name = ("I:/realman.txt".encode("utf-8"))
        ret = self.pDll.Send_TrajectoryFile(self.nSocket, name, len(name), 50)
        logging.info(f'在线编程文件下发成功：{ret}')

    def plan_num(self):
        FileLineNum = len(open(r"I:/realman.txt").readlines())
        print(FileLineNum)

        plan = ctypes.c_int()

        self.pDll.Get_Arm_Plan_Num.argtypes = [ctypes.c_int, ctypes.POINTER(ctypes.c_int)]
        result = self.pDll.Get_Arm_Plan_Num(self.nSocket, ctypes.byref(plan))
        if result == 0:
            print("Number of arm plans: ", plan.value)
        else:
            print("Error: ", result)


        while plan.value != FileLineNum:

            print(plan.value)
            self.pDll.Get_Arm_Plan_Num(self.nSocket, ctypes.byref(plan))
            time.sleep(0.05)


        time.sleep(1)
        #   停止运动
        self.pDll.Move_Pause_Cmd(self.nSocket, 1)
        self.pDll.Clear_Current_Trajectory(self.nSocket, 1)
        self.pDll.Clear_All_Trajectory(self.nSocket, 1)
        self.pDll.Move_Continue_Cmd(self.nSocket, 1)

    def close(self):
        self.pDll.Arm_Socket_Close(self.nSocket)


def main():
    arm = Arm(ip='192.168.1.19')
    # arm.tool_change()
    arm.send_file()
    arm.plan_num()
    arm.close()


if __name__ == "__main__":
    main()





